#include <mc_velocity_planning/velocity_planning_base.h>

VelocityPlanningBase::VelocityPlanningBase(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer){
    pointCloudXYZPtr_ = nullptr;
    velPub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    localPathPub_ = nh_.advertise<nav_msgs::Path>("/local_path", 10);
    LookaheadPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);
}


VelocityPlanningBase::~VelocityPlanningBase(){

}

// 设置雷达点云数据  相对于base_link
void VelocityPlanningBase::setPointCloudData(sensor_msgs::PointCloud2::Ptr cloud_ptr){
    pointCloudXYZPtr_ = msg_convert::convertToPointCloudXYZ(cloud_ptr);
}


// 检测目标点是否到达  距离判断
bool VelocityPlanningBase::IsPositionReached(Pose2D robot_pose, Pose2D goal_pose, float xy_tolerance){
    float dist = math_utils::DistancePose2D(goal_pose, robot_pose);
    cout << "[IsPositionReached] dist: " << dist << endl;
    if(dist < xy_tolerance){
        return true;
    }
    return false;
}

// 检测目标点是否到达  角度判断
bool VelocityPlanningBase::IsAngleReached(Pose2D robot_pose, Pose2D goal_pose, float yaw_tolerance){
    float theta = math_utils::DifferenceAngle(goal_pose[2], robot_pose[2]);
    cout << "[IsAngleReached] theta: " << theta << endl;
    if(fabs(theta) < yaw_tolerance){
        return true;
    }
    return false;
}

// 获取离当前位置最近的前瞻点  简单的距离计算，后期优化
bool VelocityPlanningBase::FindLookAheadPointByDistance(vector<Pose2D>& local_path, 
    Pose2D curr, Pose2D& target_point, float lookahead_distance){
    // cout << "[FindLookAheadPointByDistance] local_path size: " << local_path.size() << endl;
    if(local_path.empty())
        return false;

    // 发布本地路径
    nav_msgs::Path local_path_msg = msg_convert::ToPath(local_path, "base_link");
    localPathPub_.publish(local_path_msg);
 
    Pose2D origin_data = Eigen::Vector3f::Zero();
    for(size_t i = local_path.size()-1; i>0; --i){
        // float dis = math_utils::DistancePose2D(local_path[i], curr);
        float dis = math_utils::DistancePose2D(local_path[i], origin_data);
        cout << "[FindLookAheadPointByDistance] i: " << i
            << ", pose: " << local_path[i].transpose() 
            << ", dis: " << dis 
            << ", lookahead_distance: " << lookahead_distance
            << ", local_path size: " << local_path.size() << endl;
        if(dis < lookahead_distance){
            // cout << "[FindLookAheadPointByDistance] --> i: " << i << endl;
            target_point = local_path[i];
            geometry_msgs::PoseStamped lookahead_pose 
                = msg_convert::ToGeometryPoseStamp(target_point, "base_link");
            LookaheadPosePub_.publish(lookahead_pose);
            return true;
        }
    }
    return false;
}


// 发布空速度
void VelocityPlanningBase::PublishEmptyTwist(){
    geometry_msgs::Twist twist_msg;
    velPub_.publish(twist_msg);
}


// 发布速度
void VelocityPlanningBase::PublishTwist(Velocity2D velocity){
    geometry_msgs::Twist twist_msg = msg_convert::ToGeometryTwist(velocity);
    velPub_.publish(twist_msg);
}

// 机器人停止
bool VelocityPlanningBase::Stop(Velocity2D& cmd_vel, bool e_stop, Velocity2D curr_vel, Pose2D curr, Pose2D goal){
    // 机器人急停
    if(e_stop){
        cmd_vel[0] = 0.0;
        cmd_vel[1] = 0.0;
        cmd_vel[2] = 0.0;
    }
    else{   //按控制频率减速
        // v = v0 - at
        cmd_vel[0] = sign(curr_vel[0]) * max(0.0f, fabs(curr_vel[0]) - __max_vel_limit[0] * __controller_frequency);
        cmd_vel[1] = sign(curr_vel[1]) * max(0.0f, fabs(curr_vel[1]) - __max_vel_limit[1] * __controller_frequency);
        cmd_vel[1] = sign(curr_vel[2]) * max(0.0f, fabs(curr_vel[2]) - __max_vel_limit[2] * __controller_frequency);
    }
    return true;
}

// 旋转到目标点位姿 的周期速度
bool VelocityPlanningBase::RotateToGoal(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal){
    cmd_vel[0] = 0;
    cmd_vel[1] = 0; 
    float angle_diff = math_utils::DifferenceAngle(curr[2], goal[2]);
    // 最大减速 v^2 = v0^2 + 2as
    float max_speed_to_stop = sqrt(2 * __max_vel_limit[2] * fabs(angle_diff));
    // 最大加速限制 [__min_vel_limit ~ __max_vel_limit]
    cmd_vel[2] = min(__max_vel_limit[2], max(__min_vel_limit[2], max_speed_to_stop));
    if(angle_diff < 0){
        cmd_vel[2] = - cmd_vel[2];
    }
    return true;
}


// 获取机器人当前位姿
bool VelocityPlanningBase::GetRobotPose(geometry_msgs::PoseStamped& global_pose){
    // 尝试3次
    for(int i=0; i<3; i++){
        if(GetTFPose(global_pose, __map_frame, __base_frame, ros::Time::now())){
            return true;
        }
        ros::Duration(0.05).sleep();
    }
    return false;
}

// 获取TF坐标变换
bool VelocityPlanningBase::GetTFPose(geometry_msgs::PoseStamped& global_pose, 
    string father_frame, string child_frame, ros::Time stamp){
    return path_utils::GetTFPose(tfBuffer_, global_pose, father_frame, child_frame, stamp);
}

